#include "./can_bus.h"
#include "../device/gpio.h"
#include "../conn/proto.h"

namespace driver {

using namespace device::gpio;
using namespace utils;
using namespace conn;

using TX = PA<12>;
using RX = PA<11>;
using S = PA<15>;

// 过滤器0 过滤本机数据包
// 过滤器1 过滤IPC数据包 / 机械臂数据包
static void filter_config() {
    CAN1->FMR = CAN_FMR_FINIT;
    CAN1->FA1R = 0x00; // 过滤器先禁用

#if defined(BOARD_PIPE_LINE) || defined(BOARD_ARM)
    CAN1->FS1R = 0x03; // 0/1 过滤器开启
#else
    CAN1->FS1R = 0x01; // 0 过滤器开启
#endif

    CAN1->FM1R = 0x00; // 2个32位 标识符屏蔽
    CAN1->FFA1R = 0x00; // 关类到FIFO0

    CAN1->sFilterRegister[0].FR1 = CAN_FILTER_LOCAL_VAL;
    CAN1->sFilterRegister[0].FR2 = CAN_FILTER_LOCAL_MASK;

#if defined(BOARD_PIPE_LINE)
    CAN1->sFilterRegister[1].FR1 = CAN_FILTER_IPC_VAL;
    CAN1->sFilterRegister[1].FR2 = CAN_FILTER_IPC_MASK;
#endif

#if defined(BOARD_ARM)
    CAN1->sFilterRegister[1].FR1 = CAN_FILTER_ARM_VAL;
    CAN1->sFilterRegister[1].FR2 = CAN_FILTER_ARM_MASK;
#endif

#if defined(BOARD_PIPE_LINE) || defined(BOARD_ARM)
    CAN1->FA1R = 0x03; // 0/1 过滤器激活
#else
    CAN1->FA1R = 0x01; // 0 过滤器激活
#endif
    CAN1->FMR = 0x00;
}

CANBus::CANBus() {
    notify_frame_tx_complete();
    pkg_unlock();
    pkg_ack_unlock();

    RCC->APB2ENR |= RCC_APB2ENR_IOPAEN;
    RCC->APB1ENR |= RCC_APB1ENR_CAN1EN;

    config_float_input<RX, S>();
    config_af_pp_out<TX>();

    CAN1->MCR = CAN_MCR_INRQ +
        CAN_MCR_ABOM +
        CAN_MCR_NART;
    while ((CAN1->MSR & CAN_MSR_INAK) == 0);

    // 36 / (3 + TBS2 + TBS1) / (1 + BRP)

    constexpr uint32_t TBS2 = 2;
    constexpr uint32_t TBS1 = 1;
    constexpr uint32_t BRP = 5 ;

    CAN1->BTR = (TBS2 << 20) +
        (TBS1 << 16) +
        BRP;

    CAN1->MCR &= ~CAN_MCR_INRQ;
    while ((CAN1->MSR & CAN_MSR_INAK) != 0x00);

    filter_config();

    CAN1->IER = CAN_IER_TMEIE + CAN_IER_FMPIE0;

    NVIC_EnableIRQ(USB_HP_CAN1_TX_IRQn);
    NVIC_EnableIRQ(USB_LP_CAN1_RX0_IRQn);
}

void CANBus::write_ack(uint8_t dest, uint8_t seq) {
    uint32_t id = proto::REQ_ACK |
        (ConnInfo::TYPE_RESPONSE << 8) |
        (seq << 12) |
        (dest << 16) |
        (MY_ADDR << 20) |
        (0 << 24) |
        (1 << 28);
    write_ext_empty_frame(id);
}

void CANBus::write_empty_frame(uint32_t tir) {
    wait_frame_tx_complete();
    CAN1->sTxMailBox[0].TDTR = 0;
    CAN1->sTxMailBox[0].TIR = tir;
}

void CANBus::write_frame(uint32_t tir, const uint8_t *buf8, uint8_t len) {
    wait_frame_tx_complete();

    CAN1->sTxMailBox[0].TDTR = len;
    
    CAN1->sTxMailBox[0].TDLR = buf8[0] | 
        (buf8[1] << 8) |
        (buf8[2] << 16) |
        (buf8[3] << 24);
    
    CAN1->sTxMailBox[0].TDHR = buf8[4] |
        (buf8[5] << 8) |
        (buf8[6] << 16) |
        (buf8[7] << 24);
    
    CAN1->sTxMailBox[0].TIR = tir;
}

void CANBus::write_ext_pkg(const ConnInfo &conn_info, const uint8_t *buf_align8, uint8_t len) {
    constexpr uint32_t CLR_MASK = 0x1F << 24;
    uint8_t frame_id = 0;
    uint8_t index = 0;
    uint32_t id = conn_info.can_id();

    pkg_lock();
    do {
        uint8_t n = len - index;
        if (n > 8) {
            n = 8;
        }
        
        bool complete_flag = (index + n) >= len;

        id = id & (~CLR_MASK) | 
            (frame_id << 24) | 
            (complete_flag << 28);
        write_ext_frame(id, buf_align8 + index, n);
        frame_id ++;
        index += n;

    } while (index < len);
    pkg_unlock();
}

Error CANBus::write_ext_pkg_wait_ack(const ConnInfo &conn_info, const uint8_t *buf, uint8_t len) {
    Error err;
    pkg_ack_lock();
    clear_ack();
    for (uint8_t i = 0; i < 3; i ++) {
        write_ext_pkg(conn_info, buf, len);
        if (wait_ack()) {
            goto finish;
        }
    }
    err = ec::CAN_NOT_RECV_ACK;
finish:
    pkg_ack_unlock();
    return err;
}

void CANBus::notify_frame_tx_complete() {
    m_can_ec = (CAN1->ESR >> 4) & 0x07;
    m_flags.set_flags(FLAG_FRAME_COMPLETE);
}

void CANBus::on_frame_recv() {
    conn::CANFrame frame;
    frame.rir = CAN1->sFIFOMailBox[0].RIR;
    frame.len = CAN1->sFIFOMailBox[0].RDTR & 0x0F;

    uint32_t tmp = CAN1->sFIFOMailBox[0].RDLR;
    frame.buf[0] = tmp;
    frame.buf[1] = tmp >> 8;
    frame.buf[2] = tmp >> 16;
    frame.buf[3] = tmp >> 24;
    
    tmp = CAN1->sFIFOMailBox[0].RDHR;
    frame.buf[4] = tmp;
    frame.buf[5] = tmp >> 8;
    frame.buf[6] = tmp >> 16;
    frame.buf[7] = tmp >> 24;

    CAN1->RF0R = CAN_RF0R_RFOM0;

#if defined(BOARD_ARM)
    if (frame.is_ext()) {
        m_mq.post(frame);
    } else {
        m_arm_mq.post(frame);
    }
#else
    m_mq.post(frame);
#endif

}

}

using namespace driver;

extern "C"
void USB_LP_CAN1_RX0_IRQHandler() {
    if ((CAN1->RF0R & CAN_RF0R_FMP0) != 0) {
        canBus->on_frame_recv();
        return;
    }
}

extern "C"
void USB_HP_CAN1_TX_IRQHandler() {

    if (CAN1->TSR & CAN_TSR_RQCP0) {
        CAN1->TSR = CAN_TSR_RQCP0;
        canBus->notify_frame_tx_complete();
    }    
}

